import math
import matplotlib.pyplot as plt
import matplotlib.patches as patches
import pandas as pd
from bins import vehicleParams
from bins.tool import build_wheel_patches, rotate_and_translate, shift_points, polygon_center


class Draw:
    def __init__(self, length: int = 80, width: int = 40):
        self.draw = plt.figure(figsize=(length, width))
        self.ax = self.draw.add_axes([0, 0, 1, 1])

        # 道路参数
        self.road_length = 200  # 道路长度
        self.road_width = 3.4 * 2  # 双车道总宽度（假设每车道宽度各为 1）

        self.road = None
        # self.ax.set_xlim(0, self.road_length)
        # self.ax.set_ylim(0, self.road_width)

        # 让纵横比相同，去除坐标轴等多余信息
        self.ax.set_aspect('equal', adjustable='box')
        plt.axis('off')


    def drawRoad(self):
        self.road = patches.Rectangle(
            (0, 0),  # 左下角坐标
            self.road_length,  # 矩形长度（x 方向）
            self.road_width,  # 矩形宽度（y 方向）
            facecolor='gray',
            edgecolor='black'
        )
        self.ax.add_patch(self.road)

    def drawLine(self, height, color = "white", linewidth = 2, linestyle = "--"):
        self.ax.plot(
            [0, self.road_length],
            [height, height],
            color=color,
            linestyle=linestyle,
            linewidth=linewidth
        )

    def drawArrow(self, begin_x, begin_y):
        self.ax.arrow(
            begin_x,  # 箭头起点 X
            begin_y,  # 箭头起点 Y
            0.5,  # 箭头在 X 方向的长度
            0,  # 箭头在 Y 方向的偏移（0 表示水平箭头）
            width=0.02,  # 箭体宽度（可调整）
            head_width=0.1,  # 箭头头部宽度
            head_length=0.15,  # 箭头头部长度
            fc='white',  # 填充颜色
            ec='white'  # 边框颜色
        )

    def savefig(self, filename: str = "test"):
        plt.savefig(f"../picture/{filename}.jpg")
        # plt.show()

    def drawCar(self, begin_x, begin_y, heading = math.radians(0),
                steering = math.radians(0),
                car_type: vehicleParams.VehicleParams() = None,
                car_name: str = "Car"):
        if car_type is None:
            car_type = vehicleParams.VehicleParams()

        wheel_center_offset = car_type.vehicle_params["width"] * 0.4
        wheel_width = car_type.vehicle_params["width"] * 0.1
        wheel_len = 0.3

        rear_wheels = build_wheel_patches(axle_x=0,
                                           center_y=0,
                                           angle=0,
                                           wheel_center_offset=wheel_center_offset,
                                           wheel_len=wheel_len,
                                           wheel_wid=wheel_width)

        front_wheels = build_wheel_patches(axle_x=car_type.vehicle_params["length"]
                                                  - car_type.vehicle_params["front_wheel_to_front_bumper"]
                                                  - car_type.vehicle_params["rear_wheel_to_rear_bumper"],
                                           center_y=0,
                                           angle=steering,
                                           wheel_center_offset=wheel_center_offset,
                                           wheel_len=wheel_len,
                                           wheel_wid=wheel_width)

        self.drawCarOutline(begin_x, begin_y, heading, car_type, car_name)

        rear_wheels_global = []
        for w_pts in rear_wheels:

            w_pts_global = rotate_and_translate(w_pts, heading, begin_x, begin_y)
            rear_wheels_global.append(w_pts_global)

        front_wheels_global = []
        for w_pts in front_wheels:
            w_pts_global = rotate_and_translate(w_pts, heading, begin_x, begin_y)
            front_wheels_global.append(w_pts_global)

        # body_patch = patches.Polygon(body_global, closed=True,
        #                      facecolor='blue', edgecolor='black', linewidth=1)
        # self.ax.add_patch(body_patch)

        for wheel_pts in (rear_wheels_global + front_wheels_global):
            wheel_patch = patches.Polygon(wheel_pts, closed=True,
                                  facecolor='black', edgecolor='black', linewidth=1)
            self.ax.add_patch(wheel_patch)


    def drawCarOutline(self, begin_x, begin_y, heading = math.radians(0),
                       car_type: vehicleParams.VehicleParams() = None,
                       car_name: str = None,
                       color: str = "blue"):
        if car_type is None:
            car_type = vehicleParams.VehicleParams()

        chamfer = 0.1 * min(car_type.vehicle_params["length"], car_type.vehicle_params["width"])

        # 八个倒角的点，也就是长方形车身根据chamfer来倒角
        car_body = [(chamfer, 0), (car_type.vehicle_params["length"] - chamfer, 0),
                    (car_type.vehicle_params["length"], chamfer),
                    (car_type.vehicle_params["length"], car_type.vehicle_params["width"] - chamfer),
                    (car_type.vehicle_params["length"] - chamfer, car_type.vehicle_params["width"]),
                    (chamfer, car_type.vehicle_params["width"]),
                    (0, car_type.vehicle_params["width"] - chamfer),
                    (0, chamfer)
                    ]
        car_body = shift_points(car_body, car_type.vehicle_params["rear_wheel_to_rear_bumper"],
                                car_type.vehicle_params["width"] / 2)

        body_global = rotate_and_translate(car_body, heading, begin_x, begin_y)
        body_patch = patches.Polygon(body_global, closed=True,
                             facecolor=color, edgecolor='black', linewidth=1)
        self.ax.add_patch(body_patch)

        if car_name is not None:
            cx, cy = polygon_center(body_global)
            self.drawText(cx, cy, car_name)


    def drawText(self, x, y, text, color = 'white'):
        self.ax.text(x, y, text,
                     ha='center',       # 水平居中
                     va='center',       # 垂直居中
                     color=color,     # 文字颜色
                     fontsize=20,       # 字体大小，可自行调整
                     zorder=12 )

    def drawTrajectory(self, filename: str=None, color:str = 'white', x: list=None, y:list=None, h: list=None):
        if filename is not None:
            df = pd.read_csv(filename)
            npc_data = df.to_numpy()
        if x is None and y is None and h is None:
            x = npc_data[:, 0]
            y = npc_data[:, 1]
            h = npc_data[:, 3]
        for i in range(0, len(x)):
            colors = (0.5, 0.2, i / len(x), 0.5)
            self.drawCarOutline(begin_x=float(x[i]), begin_y=float(y[i]),heading=float(h[i]),color=colors)
            # self.drawText(float(x[i] + 0.5), float(y[i]), str(i), color)



    def initRoad(self):
        self.drawRoad()
        self.drawLine(self.road_width / 2)
        self.drawLine(0, "black", 8, "-")
        self.drawLine(self.road_width, "black", 8, "-")
        self.drawArrow(self.road_length * 0.15, self.road_width / 4)
        self.drawArrow(self.road_length * 0.15, self.road_width * 3 / 4)
        # car_x = 2.0
        # car_y = 0.3
        #
        # car_rect = patches.Rectangle(
        #     (car_x, car_y),  # 车左下角坐标
        #     car_type.vehicle_params["length"],  # 车身长度（x 方向）
        #     car_type.vehicle_params["width"],  # 车身宽度（y 方向）
        #     facecolor='blue',  # 车的填充颜色
        #     edgecolor='black',  # 车的边框颜色
        #     linewidth=1
        # )
        # self.ax.add_patch(car_rect)

"""
if __name__ == "__main__":
    draw = Draw()
    draw.drawRoad()
    draw.drawLine(draw.road_width / 2)
    draw.drawLine(0, "black", 8, "-")
    draw.drawLine(draw.road_width, "black", 8, "-")
    draw.drawArrow(draw.road_length * 0.15, draw.road_width / 4)
    draw.drawArrow(draw.road_length * 0.15, draw.road_width * 3 / 4)
    
    # draw.drawCar(1.0, 2, 90 / 180 * math.pi, 30 / 180 * math.pi)
    # draw.drawCar(56.0, 3.4 / 2, 0, 0 / 180 * math.pi, car_name="CarI")
    # draw.drawCar(53.0, 3.4 / 2, 0, 0 / 180 * math.pi, car_name="CarII")
    # draw.drawCar(50.0, 3.4 / 2, 0, 0 / 180 * math.pi, car_name="CarIII")
    # draw.drawCar(18, draw.road_width * 3 / 4, 0, 0 / 180 * math.pi, car_name="CarIX")
    # draw.drawCar(14, draw.road_width * 3 / 4, 0, 0 / 180 * math.pi, car_name="CarX")
    # draw.drawCar(10, draw.road_width * 3 / 4, 0, 0 / 180 * math.pi, car_name="CarXI")
    # draw.drawCarOutline(30, 2, 330 / 180 * math.pi, car_name="model", color="#FF0000")
    # draw.drawCar(30.0, 2, 0, 0 / 180 * math.pi, car_name="CarXII")
    # draw.drawCar(25.0, 2, 0, 0 / 180 * math.pi, car_name="CarXIII")
    # draw.drawCar(20.0, 2, 0, 0 / 180 * math.pi, car_name="CarIV")
    # draw.drawCar(15, 2, 0, 0 / 180 * math.pi, car_name="CarV")

    draw.drawCar(46, draw.road_width / 4, 0, 0 / 180 * math.pi, car_name="CarI")
    draw.drawCar(42, draw.road_width / 4, 0, 0 / 180 * math.pi, car_name="CarII")
    draw.drawCar(38, draw.road_width / 4, 0, 0 / 180 * math.pi, car_name="CarIII")

    draw.drawCar(34, draw.road_width / 4, 0, 0 / 180 * math.pi, car_name="CarIX")
    draw.drawCar(30, draw.road_width / 4, 0, 0 / 180 * math.pi, car_name="CarX")
    draw.drawCar(26, draw.road_width / 4, 0, 0 / 180 * math.pi, car_name="CarXI")
    draw.drawCar(22.0, draw.road_width / 4, 0 / 180 * math.pi, car_name="CarXII")
    draw.drawCar(18.0, draw.road_width / 4, 0, 0 / 180 * math.pi, car_name="CarXIII")
    draw.drawCar(14.0, draw.road_width / 4, 0, 0 / 180 * math.pi, car_name="CarIV")
    draw.drawCar(10, draw.road_width / 4, 0, 0 / 180 * math.pi, car_name="CarV")
    draw.savefig("十车静止队列")
"""

if __name__ == "__main__":
    draw = Draw()
    draw.initRoad()
    draw.drawTrajectory("../trajectory/test1.csv")
    draw.drawCar(46, draw.road_width / 4, 0, 0 / 180 * math.pi, car_name="CarI")
    draw.savefig("car1begin")